package com.yukefms.rule.formula.basis;

import com.yukefms.engine.running.MatchedStatus;
import com.yukefms.engine.running.RunningContext;
import com.yukefms.jointpoint.BodyPointCloud;
import com.yukefms.jointpoint.GeometryRelations;
import com.yukefms.jointpoint.TolerantEpsilon;
import com.yukefms.jointpoint.interfaces.Position;
import com.yukefms.rule.formula.interfaces.AbstractStaticBAFormula;
import com.yukefms.rule.formula.interfaces.DynamicBAFormula;
import com.yukefms.rule.formula.interfaces.StaticBAFormula;

/**
 * @Description
 * @Author Ping
 * @Date 2021/7/17 17:45
 **/
public class DoingToUnSatBAFormula extends AbstractStaticBAFormula implements DynamicBAFormula {

    private String jointType;
    /**
     * inner state variable
     */
    private boolean isDoing ;
    /**
     * important parameter: the target measurement needs to arrive.
     */
    private double targetMeasurement = 0.01;
    private double prevMeasurement ;
    private int unsatErrorCounter = 0 ;


    public DoingToUnSatBAFormula() {
    }

    public DoingToUnSatBAFormula(final String jointType) {
        super(jointType);
        this.jointType = jointType;
        this.isDoing = false ;
    }
    public DoingToUnSatBAFormula(final String jointType, double targetMeasurement) {
        super(jointType);
        this.jointType = jointType;
        this.isDoing = false ;
        this.targetMeasurement = targetMeasurement ;
    }
    @Override
    public MatchedStatus matched(BodyPointCloud bodyPointCloud, RunningContext context) {
        Position lastPosition = context.getLastPosition(jointType) ;
        if (lastPosition == null) {
            System.out.println("LAST POSITION IS NULL for "+jointType) ;
            return MatchedStatus.FALSE ;
        }
        Position currentPosition = bodyPointCloud.getPosition(jointType) ;
        // moving down.
        if (
                GeometryRelations.isHorizonalBelow(currentPosition, lastPosition) &&
            // moving down but by approaching the camera, ie. in forwarding manner.
                GeometryRelations.increasedAtZ(currentPosition, lastPosition)

        ) {
            // for the first time matched, storing current value and change the state to doing.
            if (!this.isDoing) {
                this.prevMeasurement = currentPosition.getZ() ;
                //System.out.println("New Prev:" + this.prevMeasurement) ;
                this.isDoing = true ;
                this.unsatErrorCounter = 0 ;
            }
            // if has successor , then pass to the next formula.
            if (this.successor != null) {
                return this.successor.matched(bodyPointCloud, context);
            } else {
                return MatchedStatus.TRUE;
            }
        }
        // when at state of doing, the state transition was ether to FALSE when over the tolerant unsat limit,
        // or to True , return true and change the state to not doing.
        if (this.isDoing) {

            this.unsatErrorCounter++ ;
            // if the number of unmatched time was passed the tolerant limit, then return unmatched.
            if ( this.unsatErrorCounter >= TolerantEpsilon.DOING_TRANSITION_MAX_UNSAT_ERROR) {
                this.isDoing = false ;
                this.unsatErrorCounter = 0 ;

                return MatchedStatus.FALSE;
            }
       /*     System.out.println("pre:" + prevMeasurement
                             + "cur:" + currentPosition.getZ()
                    + "DIS: " + (prevMeasurement - currentPosition.getZ())) ;*/
            if (prevMeasurement - currentPosition.getZ()  >= targetMeasurement) {
                this.isDoing = false ;
                this.unsatErrorCounter = 0 ;
                return MatchedStatus.TRUE ;
            }
        }
        // if does not matched
        return MatchedStatus.FALSE;
    }


}
